#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <math.h>

#define MASK_SIZE 4
#define IMAGE_SIZE 9
#define WOWvalue 100


// Helper functions
/**
 * Generates a template matrix.
 * @return a template matrix.
 */
unsigned char* templateM();
unsigned char* imgM();
void printM(unsigned char* matrix, int side);


float mean(unsigned char* data, int n) 
{
	float sum = 0;
	int i;
	for(i = 0; i < n; i++) {
		sum += (float)data[i];
        }
	return sum / (float)n;
}	

float standardDiv(unsigned char* data, int n, float mean) 
{
	float variance = 0;
	int i;
	for(i = 0; i < n; i++) 
	{
		variance += ((float)data[i] - mean) * ((float)data[i] - mean);
	}
	variance = variance / (float)n;
	return sqrtf(variance);	
}

float valueAt(unsigned char* m, int rowS, int x, int y) {
    unsigned char c = m[rowS * y + x];
    return (float)c;
}

/**
 * Returns a sub image of matrix m that is a rectangle who's top left corner
 * is at (x,y).
 * @param x The x coordinate of the top right corner of the new matrix.
 * @param y The y coordinate of the top right corner of the new matrix.
 * @param m The matrix to take the sample from.
 * @return The sub matrix.
 */
unsigned char* takeSample(int x, int y, unsigned char* m) 
{
       	unsigned char* s = (unsigned char*)malloc(MASK_SIZE*MASK_SIZE*sizeof(unsigned char));
	int xC  = x, yC = y, i = 0;
	for(yC = y; yC < MASK_SIZE + y; yC++) {
		for(xC = x; xC < (MASK_SIZE + x); xC++) { 
		s[i] = (unsigned char)valueAt(m, IMAGE_SIZE, xC, yC);
		i++;
		}
	}
	return s;
}

/**
 * Calculates a cross correlation.
 * @param actual The actual image.
 * @param template The image to compare the image to.
 * @param width The width of the two images.
 * @param height The height of the two images.
 * @param n_live_pixels The number of live pixels in the actual image.
 * @return The cross correlation number.
 */
float SimpleCorrValue(unsigned char* actual, unsigned char *template, 
        int width, int height, int n_live_pixels) 
//actual and template are of same size
{
    //Note: we are using n_live_pixels for number of pixels.
    //step 1: calculate mean of pixels
    float meanTemplate = mean(template, n_live_pixels);        
    float meanActual = mean(actual, n_live_pixels);
    //step 2: calculate SD of both template and reference
    float sdivActual = standardDiv(actual, n_live_pixels, meanActual);
    float sdivTemplate = standardDiv(template, n_live_pixels, meanTemplate);
    
    int j, k;
    float sum = 0;
    for(j = 0; j < height; j++) {
        for(k = 0; k < width; k++) {
            sum += 
                    (valueAt(actual, n_live_pixels, k, j) - meanActual) *
                    (valueAt(template, n_live_pixels, k, j) - meanTemplate);
            
                    
        }
    }

    return sum / (n_live_pixels * sdivActual * sdivTemplate);
}
	
float crossCorrelation(unsigned char* image, unsigned char* template,
        int* maxX, int* maxY, int width, int height, int n_live_pixels) 
{
	int x, y;
	float maxCor = -999;
	for(x = 0; x < IMAGE_SIZE - MASK_SIZE + 1; x++) {
		for(y = 0; y < IMAGE_SIZE - MASK_SIZE + 1; y++) {
                    unsigned char* actual = takeSample(x, y, image);
                    float currCor = SimpleCorrValue(actual, template, 
                            MASK_SIZE, MASK_SIZE, n_live_pixels);
                    printf("(%d,%d) %f\n",x, y, currCor);
			if(maxCor < currCor) {
				maxCor = currCor;
				*maxX = x;
				*maxY = y;
			}
		}
	}	
	printf("Best correlation found is %f.\n",maxCor);
	printf("samplre\n");
	unsigned char* d = takeSample(*maxX, *maxY, image);
	
        return maxCor;
}

int main(int argc, char* argv[]) 
{
    
	unsigned char* template = templateM();
	unsigned char* img = imgM();
       
	printf("MASK:\n");
	printM(template, MASK_SIZE);

	printf("IMAGE:\n");
        printM(img, IMAGE_SIZE);
        
	int x = -1, y = -1;
	crossCorrelation(img, template, &x, &y, IMAGE_SIZE, IMAGE_SIZE, MASK_SIZE*MASK_SIZE);
	printf("The pattern is at (%d,%d).\n",x,y);	
        
	return 0;
}

unsigned char* templateM() 
{
	unsigned char* sMatrix = 
		(unsigned char*)malloc(MASK_SIZE*MASK_SIZE * sizeof(unsigned char));
	int i;
	for(i = 0; i < MASK_SIZE*MASK_SIZE; i++) 
	{
		//if(i == (MASK_SIZE / 2)) sMatrix[i] = WOWvalue;
		if(i == 0) sMatrix[i] = 0;
	       	else sMatrix[i] = 0;
	}
	sMatrix[1] = 0; //86
	sMatrix[2] = 555; //22
	sMatrix[3] = 0; //65
	sMatrix[9] = 0; //16
	printf("Generated a sample of size %d\n",MASK_SIZE);
	return sMatrix;
}	

unsigned char* imgM() 
{

	printf("Generated an image of size %d\n",IMAGE_SIZE);
	srand(time(NULL));
	int randX = (rand() % ((IMAGE_SIZE - MASK_SIZE*2)) + MASK_SIZE);
	int randY = (rand() % ((IMAGE_SIZE - MASK_SIZE*2)) + MASK_SIZE);
	unsigned char* imgMatrix = 
		(unsigned char*)malloc(IMAGE_SIZE*IMAGE_SIZE*sizeof(unsigned char));
	int i;
	for(i = 0; i < IMAGE_SIZE*IMAGE_SIZE; i++)
	{
		if(i % IMAGE_SIZE == randX && i / IMAGE_SIZE == randY) {
			imgMatrix[i] = 0;
		}
		else imgMatrix[i] = 0;
	}
	printf("Generated an image with a peak at (%d, %d)\n",randX,randY);
	imgMatrix[4] = 0; //100
	imgMatrix[7] = 0; //65
	imgMatrix[5] = 100; //86
	imgMatrix[6] = 0; //22
	imgMatrix[23] = 0; //16
	return imgMatrix;
}

void printM(unsigned char* matrix, int side) 
{
	int i;
	for(i = 0; i < side*side; i++) {
		printf("%.0f   ", (float)matrix[i]);
		if((i+1) % side == 0 && i != 0) printf("\n");
	}
	printf("\n");
}

		

